function [ x, P, K ] = rtssmooth( xPrior, xPost, PHI, PPrior, PPost ) %#codegen
%RTSSMOOTH Rauch-Tung-Striebel固定区间平滑算法
%
% Input Arguments:
% xPrior: N*n矩阵，n为系统状态个数，N为采样个数，各采样时刻更新前的状态向量
% xPost: N*n矩阵，各采样时刻更新后的状态向量
% PHI: n*n*(N-1)矩阵，各采样时刻至下一采样时刻的状态转移矩阵
% PPrior: n*n*N矩阵，各采样时刻更新前的误差协方差矩阵
% PPost: n*n*N矩阵，各采样时刻更新后的误差协方差矩阵
%
% Output Arguments:
% x: N*n矩阵，平滑后各采样时刻的状态向量
% P: n*n*N矩阵，平滑后各采样时刻的误差协方差矩阵
% K: n*n*(N-1)矩阵，各采样时刻（除最后一个外）的平滑增益矩阵
%
% References:
% [1] 秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安: 西北工业大学出版社, 2012: 6.1.4节

[N, n] = size(xPost);
x = NaN(N, n);
P = NaN([n n N]);
K = NaN([n n (N-1)]);
x(N, :) = xPost(N, :);
P(:, :, N) = PPost(:, :, N);
for k=(N-1):-1:1
    K(:, :, k) = PPost(:, :, k) * PHI(:, :, k)' / PPrior(:, :, k+1);
    x(k, :) = xPost(k, :) + (x(k+1, :)-xPrior(k+1, :))*K(:, :, k)';
    P(:, :, k) = PPost(:, :, k) + K(:, :, k)*(P(:, :, k+1)-PPrior(:, :, k+1))*K(:, :, k)';
end
end